/*
 * Copyright (c) 2011, Fredrik Häggström
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Luleå University of Technology nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <tT.h>
#include <env.h>

#include "hsadc.h"
#include "mcpwm.h"
#include "pid.h"

#define DELTA_T		100000

uint32_t hsadc_count =0;

env_result_t hsadc_init(hsadc_t *self, void *none)
{
  ENV_DEBUG("enter hsadc init()\r\n");
  
	/* Drive ports LOW UM p.126*/
	GPIO1->FIOCLR |=	(1<<19) | 
				(1<<22) | 
				(1<<25) | 
				(1<<26) | 
				(1<<28) | 
				(1<<29);
	
	/* Set ports to output when in general mode p.123 */
	GPIO1->FIODIR |= 	(1<<19) |  	//MCOA0
				(1<<22) | 	//MCOB0
				(1<<25) | 	//MCOA1
				(1<<26) | 	//MCOB1
				(1<<28) | 	//MCOA2
				(1<<29);	//MCOB2
				
				
  /** Enable input counters (TIM1 and PWM1) */
  /* Enable power to timer1 and clock it using CCLK. */
  SC->PCONP    |= (1 << 2);
  SC->PCLKSEL0 &= ~(0x3 << 4);
  SC->PCLKSEL0 |= (0x1 << 4);

  PINCON->PINSEL3  |= (3 << 4);
  
  /* Setup timer interval. */
  TIM1->TCR = 0;
  TIM1->CTCR = (1 << 0) | (0 << 2);
  TIM1->PR = 0;

  TIM1->MCR = 0;
  TIM1->MR0 = 0xFFFFFFFF;
  
   
  
  /* Enable power to pwm1 and clock it using CCLK. */
  SC->PCONP    |= (1 << 6);
  SC->PCLKSEL0 &= ~(0x3 << 12);
  SC->PCLKSEL0 |= (0x1 << 12);

  PINCON->PINSEL4  |= (1 << 12);
  
  /* Setup timer interval. */
  PWM1->TCR = 0;
  PWM1->CTCR = (2 << 0) | (0 << 2);
  PWM1->PR = 0;
  PWM1->TC = 0;
  PWM1->MCR = 0;
  PWM1->MR0 = 0xFFFFFFFF;
  //PWM1->TCR = 1;			//Enable the counter when calibrating
  
  
  /** Enable interrupt counter (TIM0) */
  /* Enable power to timer0 and clock it using CCLK. */
  SC->PCONP    |= (1 << 1);
  SC->PCLKSEL0 &= ~(3 << 2);
  SC->PCLKSEL0 |= (1 << 2);

  /* Setup timer interval. */
  TIM0->TCR = 0;
  TIM0->CTCR = 0;
  TIM0->PR = 0;
  TIM0->MCR = (1 << 0) | (1 << 1);
  TIM0->MR0 = DELTA_T-1;
  TIM0->TC = 0;
	//NVIC_SetPriority(TIMER0_IRQn, 0); 	// Enable IRQ after Tiny Timber has started.
  //NVIC_EnableIRQ(TIMER0_IRQn);
  
	 //	Old code
  /// Enable 16,67 MHz output freq. 
  // Enable power to timer3 and clock it using CCLK. 
  //SC->PCONP    |= (1 << 23);
  //SC->PCLKSEL1 &= ~(3 << 14);
  //SC->PCLKSEL1 |= (1 << 14);

  //PINCON->PINSEL0  |= (3 << 20);
  
  // Setup timer interval. 
  /* TODO DELETE!!!!
  TIM3->TCR = 0;
  TIM3->CTCR = 0;
  TIM3->PR = 0;
  TIM3->MCR = (1 << 0) | (1 << 1);
  TIM3->MR0 = 10000000-1;
  TIM3->TC = 0;
  NVIC_SetPriority(TIMER3_IRQn, 31);
  NVIC_EnableIRQ(TIMER3_IRQn);
  TIM3->TCR = 1;
  */
	
	// Enable CLKOUT @CCLK/5 = 20MHz
	SC->CLKOUTCFG |= (0<<0) | ((5-1)<<4) | (1<<8);
	PINCON->PINSEL3 &= ~(3 << 22);
  PINCON->PINSEL3 |= (1 << 22);

  return 0;
  
}

env_result_t hsadc_calibrate0(hsadc_t *self, void *none)
{
	uint32_t calib = 0;	
	uint32_t i;
	ENV_DEBUG("hsadc0 calibrate\r\n");
	
	CUR_RESET();	// Reset the Disable latch on the ADCs so we can drive the mosfets.

	// Pull the U-leg low.
	w_leg( MC_OFF );
	v_leg( MC_OFF );
	u_leg( MC_LOW );
	

	d_delay(5000000);	// Wait 100K clock cykles so the voltage to the HSADC becomes stable.
	TIM1->TCR = 0;
	TIM0->TCR = 0;
	TIM1->TC = 0;			// Reset counter
	TIM0->TC = 0;
	TIM1->TCR = 1;		// Enable the counter
	TIM0->TCR = 1;		// Start sampling
	
	// Wait for a response from the hsadc or until twice the conversion time
	for ( i = 0 ; i < DELTA_T*2; i++)
	{
		if (TIM1->TC) break;		
	}
	
	// If no response from HSADC exit
	if (TIM1->TC == 0) return 1;
	
	
	/* Start calibrating, reset the conuters and start sampling. */
	TIM1->TCR = 0;			// Stop counters
	TIM0->TCR = 0;
	TIM1->TC = 0;				// Reset counters
	TIM0->TC = 0;
	TIM0->IR = (1<<0);	// Clear Match Flag
	TIM1->TCR = 1;			// Enable the counters
	TIM0->TCR = 1;	
	for ( i = 0 ; i < 256 ; i++ )
	{
		while (!(TIM0->IR & (1<<0))) {
			; // Wait until the conversion is done.
		}
		calib += TIM1->TC + 1;	
		TIM1->TC = 0;
		TIM0->IR |= (1<<0);		
	}	
	
	self->offset[0] = calib ;
	
	ENV_DEBUG("HSADC0 offset= ");
	env_send(calib);
	ENV_DEBUG("\r\n");
	return 0;
}


env_result_t hsadc_calibrate1(hsadc_t *self, void *none)
{
	uint32_t calib = 0;	
	uint32_t i;
	ENV_DEBUG("hsadc1 calibrate\r\n");
	CUR_RESET();	// Reset the Disable latch on the ADCs so we can drive the mosfets.

	// Pull the V-leg low.
	u_leg( MC_OFF );
	w_leg( MC_OFF );
	v_leg( MC_LOW );
	
	d_delay(5000000);	// Wait 100K clock cykles so the voltage to the HSADC becomes stable.
	PWM1->TCR = 0;
	TIM0->TCR = 0;
	PWM1->TC = 0;			// Reset counter
	TIM0->TC = 0;
	PWM1->TCR = 1;		// Enable the counter
	TIM0->TCR = 1;		// Start sampling

	
	// Wait for a response from the hsadc or until twice the conversion time
	for ( i = 0 ; i < DELTA_T*2; i++)
	{
		if (PWM1->TC) break;		
	}
	
	// If no response from HSADC exit
	if (PWM1->TC == 0) return 1;
	
	d_delay(500000);
	/* Start calibrating, reset the conuters and start sampling. */
	PWM1->TCR = 0;			// Stop counters
	TIM0->TCR = 0;
	PWM1->TC = 0;				// Reset counters
	TIM0->TC = 0;
	TIM0->IR = (1<<0);	// Clear Match Flag
	PWM1->TCR = 1;			// Enable the counters
	TIM0->TCR = 1;	
	for ( i = 0 ; i < 256 ; i++ )
	{
		while (!(TIM0->IR & (1<<0))) {
			; // Wait until the conversion is done.
		}
		PWM1->TCR = 0;	
		calib += PWM1->TC;
  	PWM1->TC = 0;
		PWM1->TCR = 1;
		TIM0->IR |= (1<<0);		
	}	
	
	self->offset[1] = calib ;
	
	ENV_DEBUG("HSADC1 offset= ");
	env_send(calib);
	ENV_DEBUG("\r\n");
	
	return 0;
}

env_result_t hsadc_start(hsadc_t *self, void *none)
{
	PWM1->TCR = 0;			// Stop counters
	TIM1->TCR = 0;hsadc_count++;
	TIM0->TCR = 0;
	PWM1->TC = 0;				// Reset counters
	TIM0->TC = 0;
	TIM1->TC = 0;
	TIM0->IR = (1<<0);	// Clear Match Flag
	PWM1->TCR = 1;			// Enable the counters
	TIM1->TCR = 1;
	TIM0->TCR = 1;
	NVIC_SetPriority(TIMER0_IRQn, 0);
  NVIC_EnableIRQ(TIMER0_IRQn);
 	
 	return 0;
}

env_result_t hsadc_send(int32_t val)
{
  if (val < 0 ) 
  {
    val = -val;
    ENV_DEBUG("-");
  }
  if ((val / 10) > 0) {
    hsadc_send(val / 10);
  }
  
  
    while ((UART1->FIFOLVL & (0xf << 8)) == (0xf << 8)) 
    {
			/* Busy wait for buffer space to become free. */
    }
    
    UART1->THR = (val%10) + '0';
    
    
  return 0;
}
 

void TIMER0_IRQHandler(void) 
{
  int32_t temp2;
  //int32_t temp;
  tt_interrupt_mark();
  
	TIM0->IR = (1 << 0); // Clear interrupt flag
	/*
	// Get value from HSADC0
	TIM1->TCR = 0;
  temp1 = TIM1->TC;
  TIM1->TC = 0;	
	TIM1->TCR = 1;	
	*/
	// Get value from HSADC1
	PWM1->TCR = 0;	
	temp2 = PWM1->TC;
  PWM1->TC = 0;
	PWM1->TCR = 1;

	//hsadc.value[0] += (temp1<<8) - hsadc.value[0]/8 - hsadc.offset[0];
	//hsadc.value[1] += (temp2<<8) - hsadc.value[1]/8 - hsadc.offset[1];  //with low pass filter filter
	
	hsadc.value[1] = temp2 - hsadc.offset[1]/256;
	//pid_run( &pid, &hsadc.value[1]);
	//if (hsadc_count > 300 )
	//{
		
		//TT_ASYNC(&pid, pid_run, (void *)(&hsadc.value[1]));
	TT_ASYNC(&pid, pid_run, TT_ARGS_NONE);
		
	//}
	//hsadc_count++;	
	
	
	tt_schedule(1);
}
